// Daniel Shiffman
// <http://www.shiffman.net>

// A Thread using receiving UDP to receive images

import java.awt.image.*; 
import javax.imageio.*;

PImage cvideo, dvideo;
ReceiverThread thread;

int numberOfTrees = 5;

boolean isObtainedPointForTree = false;
int[] x_points = new int[numberOfTrees];
int[] y_points = new int[numberOfTrees];

float[] depths = new float[numberOfTrees];

void setup() {
  size(960,480);
  cvideo = createImage(640,480,RGB);
  dvideo = createImage(320,240,RGB);
  thread = new ReceiverThread();
  thread.start();
  //imageMode(CORNERS);
}

 void draw() {
  if (thread.availableC()) {
    cvideo = thread.getCImage();
  }
  
  if (thread.availableD()) {
    dvideo = thread.getDImage();
  }
  
  savePointsForTree();
  
  saveDepthForTree();
  
  
  // Draw the image
  background(0);
  image(cvideo, 0, 0, cvideo.width, cvideo.height); 
  image(dvideo, cvideo.width, 0, dvideo.width, dvideo.height); 
  
  drawBoxForDepth();
}

//Get the point at first time
 void savePointsForTree() {   
   if (isObtainedPointForTree == false) {         
      for(int i = 0; i < numberOfTrees; ++i) {  
        x_points[i] = (int)random(0, 320);
        y_points[i] = (int)random(0, 240);    
      }     
      isObtainedPointForTree = true;
   }
 }
 
 //Save realtime depth from depth video
 void saveDepthForTree() {
    for(int i = 0; i < numberOfTrees; ++i) {  
      int x = x_points[i];
      int y = y_points[i];
      color c = dvideo.get(x, y);
      float r = red(c);
      float g = green(c);
      float b = blue(c);
      depths[i] = (r + g + b) / 3; 
    }     
 }
  
 void drawBoxForDepth() {
   for(int i = 0; i < numberOfTrees; ++i) {  
      int x = x_points[i] * 2;
      int y = y_points[i] * 2;
      float depth = depths[i];
      
      //print(depth);
      rect(x, y, depth, depth);
      
    }     
 }
 

